/*
 * @Description: 使用卡尔曼滤波器对速度滤波
 * @Author: Dryad
 * @Date: 2019-07-05 12:17:20
 */
#pragma once
#include "./Kalman_Filter.hpp"

namespace Kalman_Filter_Velocity_Namespace
{
union Input_Control_Union {
    struct
    {
        float v1; /* 1号轮线速度(mm/s) */
        float v2; /* 2号轮线速度(mm/s) */
        float v3; /* 3号轮线速度(mm/s) */
        float v4; /* 4号轮线速度(mm/s) */
    };            /* 控制量结构体 */
    float data[4];
};
union Sys_State_Union {
    struct
    {
        float v_x; /* 横向速度(mm/s) */
        float v_y; /* 纵向速度(mm/s) */
        float w;   /* 角速度(rad/s) */
    };             /* 状态量结构体 */
    float data[3];
};

void Init(const float v_noise, const float z_rate_noise, const float wheel_radius, const float distance_x, const float distance_y); /* 初始化矩阵，输入量为控制噪声和测量噪声 */
void Kalman_Filter_Velocity(const union Input_Control_Union &input,                                                                 /* 输入控制量 */
                            const float gyro_z_rate,                                                                                /* 陀螺仪的角速度测量量(rad/s) */
                            union Sys_State_Union &sys_state,                                                                       /* 计算返回得到的状态量 */
                            arm_matrix_instance_f32 &covariance                                                                     /* 计算返回得到的协方差矩阵 */
);                                                                                                                                  /* 对速度进行滤波 */

} // namespace Kalman_Filter_Velocity_Namespace
